#include "Chassis.h"
#include "gpio.h"
#include "Action.h"
#include "user_math.h"
#include "bsp_buzzer.h"







/*机器人坐标系，面向小屏幕，向前x，右y，上yaw*/
/*          
               x
               |
                --y


*/

static int8_t Chassis_SetCtrl(Chassis_t *c,CMD_t *ctrl){
	if (c == NULL) return CHASSIS_ERR_NULL; /*主结构体不能为空 */
  if (ctrl->C_cmd.type== c->ctrl && ctrl->C_cmd.mode== c->mode) return CHASSIS_OK; /*模式未改变直接返回*/
  //此处源代码处做了pid的reset 待添加
   c->ctrl =ctrl->C_cmd.type;
	 c->mode =ctrl->C_cmd.mode;
   
   return CHASSIS_OK;
}    //设置控制模式



static int8_t Chassis_SetPitAngle(Chassis_t *c,CMD_t *ctrl){
	if(c ==NULL)return CHASSIS_ERR_NULL;
	
		if(ctrl->status[0] ==0)//
		{
			//
			c->hopemotorout.pit6020_target = 32.8f;//相机朝框
			c->pit6020_fback = c->motorfeedback.rotor_pit6020angle;
		}
		else
		{
			c->hopemotorout.pit6020_target =190.0f;
			c->pit6020_fback = c->motorfeedback.rotor_pit6020angle;
		}
	return 1;
}


static int8_t Sick_AvoidBall(Chassis_t *c){
	  if(c==NULL) return CHASSIS_ERR_NULL;
	if(c->sick_dis[0] < 350.0f) 		//判断前面有障碍物需要避障此时Y不动
	{
	 c->move_vec.Vy =0;
		
	if(c->sick_dis[1] > c->sick_dis[2])  //某边的空余量远大于另一边 往空余量走
		c->move_vec.Vx = 800;
	else if(c->sick_dis[2] > c->sick_dis[1])
		c->move_vec.Vx = -800;
  }
	else ;
// 
	return 1;
}
//该函数用来更新can任务获得的电机反馈值
int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
  if (c == NULL) return CHASSIS_ERR_NULL;
  if (can == NULL) return CHASSIS_ERR_NULL;
  for (uint8_t i = 0; i < 4; i++) 
 {
		c->motorfeedback.rotor_rpm3508[i] = can->motor.motor3508.as_array[i].rotor_speed;
		c->motorfeedback.rotor_current3508[i] = can->motor.motor3508.as_array[i].torque_current;
 }
 
 
   c->motorfeedback.rotor_pit6020angle = can->motor.pit6020.as_array[0].rotor_angle;
   c->motorfeedback.rotor_pit6020rpm = can->motor.pit6020.as_array[0].rotor_speed;
 
   c->motorfeedback.rotor_gimbal_yawangle = can->motor.chassis6020.as_array[0].rotor_angle;
   c->motorfeedback.rotor_gimbal_yawrpm = can->motor.chassis6020.as_array[0].rotor_speed;
 
   c->motorfeedback.rotor_gimbal_pitchangle = can->motor.chassis6020.as_array[1].rotor_angle;
   c->motorfeedback.rotor_gimbal_pitchrpm = can->motor.chassis6020.as_array[1].rotor_speed;
 
   c->sick_dis[0] = can->sickfed.raw_dis[0];
   c->sick_dis[1] = can->sickfed.raw_dis[1];
   c->sick_dis[2] = can->sickfed.raw_dis[2];
  return CHASSIS_OK;
}


int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq)
{
   if (c == NULL) return CHASSIS_ERR_NULL;

  c->param = param; 	/*初始化参数 */

		 for(int i =0 ; i < 4 ; i++)
	{
		   PID_init(&(c->pid.chassis_3508VecPID[i]),PID_POSITION_D,&(c->param->M3508_param));  //带D项滤波
	}
  
		  PID_init((&c->pid.chassis_pitAngle6020),PID_POSITION,&(c->param->C6020pitAngle_param));//尝试位置控制角度
	
	    PID_init((&c->pid.chassis_pitOmega6020),PID_POSITION,&(c->param->C6020pitOmega_param));
	
		 PID_init((&c->pid.chassis_gimbal_yawAnglePID),PID_POSITION,&(c->param->Gimbal_yawAngle_param));//尝试位置控制角度
	
	    PID_init((&c->pid.chassis_gimbal_yawOmegaPID),PID_POSITION,&(c->param->Gimbal_yawOmega_param));

	    PID_init((&c->pid.chassis_gimbal_pitchAnglePID),PID_POSITION,&(c->param->Gimbal_pitchAngle_param));//尝试位置控制角度
	
	    PID_init((&c->pid.chassis_gimbal_pitchOmegaPID),PID_POSITION,&(c->param->Gimbal_pitchOmega_param));
		
	    PID_init(&(c->pid.chassis_NaviVxPID),PID_POSITION,&(c->param->NaviVx_param));
	    
	    PID_init(&(c->pid.chassis_NaviVyPID),PID_POSITION,&(c->param->NaviVy_param));
	
	    PID_init(&(c->pid.chassis_NaviWzPID),PID_POSITION,&(c->param->NaviVw_param));
	
	    PID_init(&(c->pid.sick_CaliforYPID),PID_POSITION,&(c->param->Sick_CaliYparam));
	  
	    PID_init(&(c->pid.sick_CaliforXPID),PID_POSITION,&(c->param->Sick_CaliXparam));
	
	    LowPassFilter2p_Init(&(c->filled[0]),target_freq,80.0f);  //给角加速度做滤波
	
	
	    LowPassFilter2p_Init(&(c->filled[1]),target_freq,80.0f); //给w 做滤波
	
	    LowPassFilter2p_Init(&(c->filled[2]),target_freq,80.0f);  //给y做滤波
	
	    LowPassFilter2p_Init(&(c->filled[3]),target_freq,80.0f); //给x 做滤波
			
		//
	
	
	 return CHASSIS_OK;
}


void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw)  //底盘逆运动学的解算
{		
		c->hopemotorout.OmniSpeedOut[0] = -Vx+Vy+Vw;//右前
		c->hopemotorout.OmniSpeedOut[1] = -Vx-Vy+Vw;//右后
		c->hopemotorout.OmniSpeedOut[2] = Vx-Vy+Vw;//左后
		c->hopemotorout.OmniSpeedOut[3] = Vx+Vy+Vw;//左前
}
bool isArrive(fp32 real_pos,fp32 target_pos,fp32 mistake)
{
	uint16_t xArrive = 0, yArrive = 0;
	xArrive = abs_float_double(real_pos,target_pos) < mistake ? 1:0;
	yArrive = abs_float_double(real_pos,target_pos) < mistake ? 1:0;
	if(xArrive && yArrive) return true;
	else return false;
}

int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
{
	if(c ==NULL)  return CHASSIS_ERR_NULL;
	if(ctrl ==NULL)  return CHASSIS_ERR_NULL;
	   
	
	
	
	  Chassis_SetCtrl(c,ctrl);
	
      Chassis_SetPitAngle(c,ctrl);
	 
	 //此处对imu加滤波做修正
	  c->pos088.bmi088.filtered_gyro.z =LowPassFilter2p_Apply(&(c->filled[0]),c->pos088.bmi088.gyro.z);
	  
	switch (c->ctrl)
	{
	case RC:
	//在cmd里对数据进行处理 包括方向和油门
	  
	//6000为全向轮的倍率,遥控器坐标系和机器人坐标系不同
	      c->move_vec.Vw = ctrl->Vw*6000;
		  c->move_vec.Vx = -ctrl->Vy*6000;
		  c->move_vec.Vy = ctrl->Vx*6000;
		  c->hopemotorout.pit6020_target =165.0f;//大yaw


          c->hopemotorout.gimbal_yawtarget = 180.0f;//小yaw

		  c->hopemotorout.gimbal_pitchtarget = map_fp32(ctrl->poscamear,-1,1,50.0f,150.0f);//小pitch
	  //  c->hopemotorout.gimbal_pitchtarget =50;
	


		 HAL_GPIO_WritePin(FlagForUpper_GPIO_Port,FlagForUpper_Pin,GPIO_PIN_RESET);
	    break;

	case CAMERA_PICK:break;
	case MID_NAVI:
//		  //这套是全向轮的方向
	    c->move_vec.Vw =ctrl->C_navi.wz *2000;
	    c->move_vec.Vx =ctrl->C_navi.vx *2000;
	    c->move_vec.Vy =-ctrl->C_navi.vy *2000;
	
	    c->move_vec.Vw =LowPassFilter2p_Apply(&(c->filled[1]),c->move_vec.Vw);
	    c->move_vec.Vy =LowPassFilter2p_Apply(&(c->filled[2]),c->move_vec.Vy);
	    c->move_vec.Vx =LowPassFilter2p_Apply(&(c->filled[3]),c->move_vec.Vx);
	
	    c->move_vec.Vw =PID_calc(&(c->pid.chassis_NaviWzPID),c->pos088.bmi088.gyro.z,c->move_vec.Vw);
	    c->move_vec.Vx =PID_calc(&(c->pid.chassis_NaviVxPID),c->pos088.bmi088.accl.y,c->move_vec.Vx);
	    c->move_vec.Vy =PID_calc(&(c->pid.chassis_NaviVyPID),c->pos088.bmi088.accl.x,c->move_vec.Vy);
	
	   if(ctrl->status[5] ==1)
		 {
			c->move_vec.Vw = c->move_vec.Vw * 0.8f;
			c->move_vec.Vx = c->move_vec.Vx * 0.5f; 
			c->move_vec.Vy = c->move_vec.Vy * 0.5f; 	 
		 }
		 if(ctrl->status[6] ==1)
		 {
			 Sick_AvoidBall(c); 
		 }
	    break;
	case SICK_CAILING:
		break;
	}

	switch (c->mode)
	{
	case RELAXED:
				c->move_vec.Vx =0;
				c->move_vec.Vy =0;
				c->move_vec.Vw =0;
		break;
	case NORMAL:
		 break;
	case GYRO_STAY:
		   c->move_vec.Vw = c->move_vec.Vw +c->pos088.bmi088.gyro.z *2000;
	   break;
	case PUT:
	 break;
			 
}
	
   //电机速度限幅

//  abs_limit_fp(&c->move_vec.Vx,2000.0f);

//  abs_limit_fp(&c->move_vec.Vy,2000.0f);

//  abs_limit_fp(&c->move_vec.Vw,2000.0f);
  Chassis_speed_calculate(c,c->move_vec.Vx,c->move_vec.Vy,c->move_vec.Vw);
	
	
		for (uint8_t i = 0 ; i <4 ; i++)
	{
		 c->final_out.final_3508out[i] = PID_calc(&(c->pid.chassis_3508VecPID[i]),c->motorfeedback.rotor_rpm3508[i],c->hopemotorout.OmniSpeedOut[i]);
	 
		 out->motor3508.as_array[i] = c->final_out.final_3508out[i];
	}
	
//	 c->vofa_send[0] =c->hopemotorout.OmniSpeedOut[0];
//	 c->vofa_send[1] =c->motorfeedback.rotor_rpm3508[0];
//	 c->vofa_send[2] =c->hopemotorout.OmniSpeedOut[1];
//	 c->vofa_send[3] =c->motorfeedback.rotor_rpm3508[1];
//	 c->vofa_send[4] =c->hopemotorout.OmniSpeedOut[2];
//	 c->vofa_send[5] =c->motorfeedback.rotor_rpm3508[2];
//	 c->vofa_send[6] =c->hopemotorout.OmniSpeedOut[3];
//	 c->vofa_send[7] =c->motorfeedback.rotor_rpm3508[3];
	return CHASSIS_OK;

}

